import numpy as np
import cv2
import cv2.aruco as aruco

def get_mark_pixel(image):
    image_gray = cv2.cvtColor(image, cv2.COLOR_BGR2GRAY)
    # 定义ArUco字典
    aruco_dict = aruco.getPredefinedDictionary(aruco.DICT_7X7_1000)
    # 初始化检测器参数
    parameters = aruco.DetectorParameters()
    # 创建检测器对象
    detector = aruco.ArucoDetector(aruco_dict, parameters)

    corners, ids, rejected = detector.detectMarkers(image_gray)

    # print(corners[0][0,0,:])
    # cv2.circle(image, (int(corners[0][0,0,0]), int(corners[0][0,0,1])), 2, (255,0,0), 5)
    # cv2.namedWindow("img",0)
    # cv2.resizeWindow("img", 640, 480)
    # cv2.imshow("img", image)
    # cv2.waitKey(0)

    return corners[0][0,0,:].tolist()

if __name__ == "__main__":
    M = np.array([[-6.95823889e-02, 1.01480651e-01, -2.04737072e+02],
                    [ 1.01805860e-01,  6.92940154e-02, -3.12014841e+02],
                    [-5.42101086e-20,  1.08420217e-19,  1.00000000e+00]])
    
    pixel = "C:/Users/malia/Desktop/Image_20250604144439980.bmp"

    image = cv2.imread(pixel)

    image_gray = cv2.cvtColor(image, cv2.COLOR_BGR2GRAY)
    # 定义ArUco字典
    aruco_dict = aruco.getPredefinedDictionary(aruco.DICT_7X7_1000)
    # 初始化检测器参数
    parameters = aruco.DetectorParameters()
    # 创建检测器对象
    detector = aruco.ArucoDetector(aruco_dict, parameters)

    corners, ids, rejected = detector.detectMarkers(image_gray)

    print(corners[0][0,0,:])

    temp = np.ones((3, 1))
    temp[:2, :] = corners[0][0,0,:].reshape((2,1))

    mark_pixel = np.asarray(temp).reshape((-1, 1))

    print(M @ mark_pixel)
